#include <chrono>
#include <memory>
#include <string>
#include <iostream>
#include <fstream>
#include <json/json.h>

#include <Eigen/Dense>
#include <Eigen/Core>
#include <mavros_msgs/msg/state.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <mavros_msgs/srv/set_mode.hpp>
#include <mavros_msgs/srv/command_bool.hpp>
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <std_msgs/msg/int32.hpp>
using namespace std::chrono_literals;

class StaticTransformPublisher : public rclcpp::Node
{
public:
    StaticTransformPublisher(std::string name = "drone")
    : Node("static_transform_publisher"),namespace_(name)
    {
       initialize_parameters();

        RCLCPP_INFO(this->get_logger(), "Static transform publisher started");
        tf_broadcaster_.resize(num_uavs_);
        multi_pose_.resize(0);

        // 创建无人机之间的相对位姿订阅者，进行位姿的初始化处理
        read_params();
        
        for (int i = 0; i < num_uavs_; ++i) {
            tf_broadcaster_[i] = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        }
        sim_num_publisher_ = this->create_publisher<std_msgs::msg::Int32>("/sim/global/num", 10);
        // 设置定时器，每秒发布坐标变换
        transform_timer_ = this->create_wall_timer(
            30ms, std::bind(&StaticTransformPublisher::broadcast_transforms, this));
    }

    void read_params() {
        std::ifstream ifs("/home/nvidia/soft_simulate/src/uav_swarms/config/settings.json");
        Json::Value file_content;
        ifs >> file_content;

        const Json::Value vehicles = file_content["Vehicles"];
        for (const auto& droneName : vehicles.getMemberNames()) {
            geometry_msgs::msg::PoseStamped posestamped;

            posestamped.header.frame_id = "map";
            posestamped.header.stamp = this->get_clock()->now();
            posestamped.pose.position.x = vehicles[droneName]["X"].asDouble();
            posestamped.pose.position.y = vehicles[droneName]["Y"].asDouble();
            posestamped.pose.position.z = vehicles[droneName]["Z"].asDouble();
            
            posestamped.pose.orientation.x = 0;
            posestamped.pose.orientation.y = 0;
            posestamped.pose.orientation.z = 0;
            posestamped.pose.orientation.w = 1;
            RCLCPP_INFO(this->get_logger(), "read params successfully");
            RCLCPP_INFO(this->get_logger(), "x: %f, y:%f, z:%f", posestamped.pose.position.x,posestamped.pose.position.y,posestamped.pose.position.z);
            multi_pose_.push_back(posestamped);
        }
        num_uavs_ = multi_pose_.size();

        RCLCPP_INFO(this->get_logger(), "num uavs: %d", num_uavs_);
    }

    void initialize_parameters() {
        this->declare_parameter<int>("vehicle_id", 0);  // 声明为字符串类型
        this->declare_parameter<std::string>("uav_namespace", "uav_swarms");
        this->declare_parameter<std::string>("vehicle_type", "drone");
        this->declare_parameter<int>("vehicle_num", 3);

        vehicle_type = this->get_parameter("vehicle_type").as_string();
        namespaces = this->get_parameter("uav_namespace").as_string();
        vehicle_id_ = std::to_string(this->get_parameter("vehicle_id").as_int());  // 获取为字符串

        num_uavs_ = this->get_parameter("vehicle_num").as_int();  // 确保 vehicle_num 是整数

        RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle Type: %s", vehicle_type.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle ID: %s", vehicle_id_);
        RCLCPP_INFO(this->get_logger(), "Vehicle Num: %d", num_uavs_);
    }
private:
    void broadcast_transforms()
    {
        // 发布无人机数量
        std_msgs::msg::Int32 msg;
        msg.data = num_uavs_;
        sim_num_publisher_->publish(msg);
        // 坐标系间转换，发布无人机之间的相对位姿
        for (size_t i = 0; i < num_uavs_; i++)
        {
            geometry_msgs::msg::TransformStamped camera_transform;
            camera_transform.header.stamp = this->get_clock()->now();
            camera_transform.header.frame_id = "map";
            camera_transform.child_frame_id = vehicle_type + std::to_string(i) + "_local_map";
            camera_transform.transform.translation.x = multi_pose_[i].pose.position.x;
            camera_transform.transform.translation.y = multi_pose_[i].pose.position.y;
            camera_transform.transform.translation.z = multi_pose_[i].pose.position.z;
            
            camera_transform.transform.rotation.x = multi_pose_[i].pose.orientation.x;
            camera_transform.transform.rotation.y = multi_pose_[i].pose.orientation.y;
            camera_transform.transform.rotation.z = multi_pose_[i].pose.orientation.z;
            camera_transform.transform.rotation.w = multi_pose_[i].pose.orientation.w;
            tf_broadcaster_[i]->sendTransform(camera_transform);
        }
    }

    rclcpp::TimerBase::SharedPtr transform_timer_;

    std::vector<std::shared_ptr<tf2_ros::TransformBroadcaster>> tf_broadcaster_;

    // 无人机的相对位姿的订阅节点，用于发布无人机的相对位姿
    std::vector<rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr> relative_pose_sub_;
    // 订阅无人机的相对位姿
    std::vector<geometry_msgs::msg::PoseStamped> multi_pose_; 
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sim_num_publisher_;
    std::vector<bool> init_flags_;
    // 当前的无人机的状态数量
    int num_uavs_;
    std::string namespace_;
    std::string vehicle_type;
    std::string namespaces;
    int vehicle_num;
    std::string vehicle_id_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    // 无人机模式和起飞订阅者
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client_;
    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;

    // 状态发布的定时器
    rclcpp::TimerBase::SharedPtr timer_;
    // 订阅当前的无人机的状态
    mavros_msgs::msg::State current_state_;
    // 进行状态的更行
    rclcpp::Time last_request_;
    
};